#pragma once
#include <iostream>
#include <time.h>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <string>
#include <unistd.h>
#include <sys/time.h>
#include <thread>
#include <termio.h>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Eigen>
using namespace std;
using namespace cv;
using namespace Eigen;

namespace Horizon{
    enum class CameraMode{
        MonnocularCamera,
        DriveFreeMonnocularCamera,
        BinocularCamera,
        DriveFreeBinocularCamera
    };

    class Speed
    {
    public:
        Speed():rate(0.0),direction(0){}
        double        rate;// 速率
        int      direction;// 方向，-1为逆时针，0为不转，1为顺时针转
    };


    // 返回当前时间
    double now()
    {
        timeval tv;
        gettimeofday(&tv, NULL);
        // 因为上一个函数并非完全精确，主要误差来自于tv._uesc，所以除1000000使得误差最小。
        return (double)tv.tv_sec + (double)tv.tv_usec / 1000000;
    }
    /*
    * @brief:  位姿的实现方式
    *
    * @author: 西北工业大学陀螺模块负责人   
    * 
    */
    class GimbalPose
    {
    public:
        float  pitch;
        float  yaw;
        float  roll;
        double timestamp;
        // 初始化函数
        GimbalPose(float pitch = 0.0, float yaw = 0.0, float roll = 0.0)
        {
            this->pitch     = pitch;
            this->yaw       = yaw;
            this->roll      = roll;
        }
        // 左值
        GimbalPose operator=(const GimbalPose& gm)
        {
            this->pitch     = gm.pitch;
            this->yaw       = gm.yaw;
            this->roll      = gm.roll;
            this->timestamp = gm.timestamp;
            return *this;
        }

        GimbalPose operator=(const float init_value)
        {
            this->pitch     = init_value;
            this->yaw       = init_value;
            this->roll      = init_value;
            this->timestamp = now();
            return *this;
        }

        friend GimbalPose operator-(const GimbalPose& gm1, const GimbalPose gm2)
        {
            GimbalPose temp{};
            temp.pitch     = gm1.pitch - gm2.pitch;
            temp.yaw       = gm1.yaw   - gm2.yaw;
            temp.roll      = gm1.roll  - gm2.roll;
            temp.timestamp = now();
            return temp;
        }

        friend GimbalPose operator+(const GimbalPose& gm1, const GimbalPose gm2)
        {
            GimbalPose temp{};
            temp.pitch     = gm1.pitch + gm2.pitch;
            temp.yaw       = gm1.yaw   + gm2.yaw;
            temp.roll      = gm1.roll  + gm2.roll;
            temp.timestamp = now();
            return temp;
        }

        friend GimbalPose operator*(const GimbalPose& gm, const float k)
        {
            GimbalPose temp{};
            temp.pitch     = gm.pitch * k;
            temp.yaw       = gm.yaw   * k;
            temp.roll      = gm.roll  * k;
            temp.timestamp = now();
            return temp;
        }

        friend GimbalPose operator*(const float k, const GimbalPose& gm)
        {

            GimbalPose temp{};
            temp.pitch     = gm.pitch * k;
            temp.yaw       = gm.yaw   * k;
            temp.roll      = gm.roll  * k;
            temp.timestamp = now();
            return temp ;
        }

        friend GimbalPose operator/(const GimbalPose& gm, const float k)
        {
            GimbalPose temp{};
            temp.pitch     = gm.pitch / k;
            temp.yaw       = gm.yaw   / k;
            temp.roll      = gm.roll  / k;
            temp.timestamp = now();
            return temp;
        }

        friend std::ostream& operator<<(std::ostream& out, const GimbalPose& gm)
        {
            out << "[pitch : " << gm.pitch << ", yaw : " << gm.yaw << "]";
            return out;
        }
    };

    // 装甲板的类
    class Armor {
    public:
        Armor():distance_(0),id_(0){};
        Armor(float distance,int id):distance_(distance),id_(id){};
    public:
        Point3f  three_equal_point;// 和中心点判断朝向的三等分点
        Point3f            center_;// 中心点
        float            distance_;// 装甲板的距离，相对于相机坐标系,单双目均可，主单目
        GimbalPose       cur_pose_;// 装甲板当前的位姿
        double        h_yaw_angle_;// 打击陀螺时判断的角度
        double                area;// 装甲板的二维坐标
        cv::Point3f  left_up_,right_up_,right_down_,left_down_;// 四个灯条的三维坐标，双目用
        int id_;// 装甲板的id
        double h_time_stamp_ = now();// 时间戳
    public:
        
    };
    /*
    * @brief:  相机坐标系转云台坐标系
    *
    * @parma:  gm是当前云台的偏转角，pos是要转换到云台的坐标系
    * 
    * @author: 李黔琦
    *  
    */ 
    Vector3f cam2ptz(GimbalPose &gm,Vector3f &pos){
        pos = pos.transpose();

        float x1,y1,z1;
        //调的时候再改数
        x1 = 0.0f;//应该是不用动
        y1 = -0.04f;//向下移动，作减法，负数
        z1 = -0.06f;//向后移动，作减法，负数
        pos[0] = pos[0] + x1;
        pos[1] = pos[1] + y1;
        pos[2] = pos[2] + z1;
        Matrix3f pitch_rotation_matrix_;
        Matrix3f yaw_rotation_matrix_;

        gm.pitch = (gm.pitch*CV_PI)/180;
        gm.yaw = (gm.yaw*CV_PI)/180;

        pitch_rotation_matrix_
        <<
        1,              0,              0,
        0,  cos(gm.pitch),  sin(gm.pitch),
        0,  -sin(gm.pitch), cos(gm.pitch);

        yaw_rotation_matrix_
        <<
        cos(gm.yaw),  0,  sin(gm.yaw),
        0,            1,            0,
        -sin(gm.yaw), 0,  cos(gm.yaw);

        Vector3f t_pos_ = (yaw_rotation_matrix_ * pitch_rotation_matrix_)* pos;

    }
}
